package sma.iss;

import java.io.File;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;
import java.rmi.RemoteException;
import java.text.DateFormat;
import java.text.DecimalFormat;
import java.text.NumberFormat;
import java.text.SimpleDateFormat;
import java.util.Arrays;
import java.util.Collection;
import java.util.Collections;
import java.util.Date;
import java.util.HashMap;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Locale;
import java.util.Map;
import java.util.Set;
import java.util.SortedMap;
import java.util.TreeMap;
import java.util.TreeSet;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.ThreadFactory;
import java.util.concurrent.atomic.AtomicInteger;

import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import sma.interf.ICalculateIss;
import sma.interf.SyncCalculateImpl;
import sma.iss.Position.Origin;
import sma.iss.calc.CalcPower;
import sma.iss.calc.PathOptimizer;
import sma.iss.file.CsvIss;
import sma.iss.remote.ParallelMgr;
import sma.iss.remote.ThreadCalculator;
import sma.iss.util.AngleUtil;
import sma.iss.util.IssException;
import sma.nasa.ConstraintsChecker;
import au.com.bytecode.opencsv.CSVReader;
import au.com.bytecode.opencsv.CSVWriter;

/**
 * Several turn of ISS to find the good way. For one beta angle
 * 
 * @author smarsoll
 * 
 */
public class DynamicModel {

	/** Logger for this class. */
	protected final static Logger LOGGER = LoggerFactory.getLogger(DynamicModel.class);

	/** nb rot, should be 2 + 8. */
	protected static final int NB_ROTERS = 10;

	/** To manage remote calculators. */
	private ParallelMgr parallelMgr = new ParallelMgr();
	/** internal calculator. */
	protected ICalculateIss issCalculator;
	DynamicModel() {
		super();
		try {
			issCalculator = new SyncCalculateImpl();
		} catch (RemoteException e) {
			LOGGER.error("", e);
		}
	}

	/** if we can do it in this time then it's possible in more time */
	public static final double TIME_REDUCER = 0.999999;

	static {
		if (NB_ROTERS != 10) {
			LOGGER.warn("NB_ROTERS is not 10 !");
		}
	}

	Date creationDate = new Date();

	private static final String DELIM = "_";
	public String getSimulationName() {
		DateFormat dateFormat = new SimpleDateFormat("MMddHHmmss");
		return "b" + DELIM + beta + DELIM + "y" + DELIM + yaw + DELIM + dateFormat.format(creationDate);
	}

	/** tolerance */
	protected static final double EPS = 1e-9;
	/** Time (in seconds) between two consecutive evaluation points. */
	// static double TIME_PER_STATE = 60.0;

	protected double beta = -2000000;
	protected double yaw = -10.0;
	
	/** Number of Position in advance. */
	public int nbAdvance = 1;
	protected int currentNbAdcance = nbAdvance;

	/** the calculator that calculate the indicator to maximize. */
	CalcPower calc = new CalcPower();

	/** All positions to calculate, in Map that keep initial order. */
	Map<Integer, Position> positions = new LinkedHashMap<Integer, Position>();
	/** for each minute the list of best positions */
	Map<Integer, Collection<Position>> mapListPos = new LinkedHashMap<Integer, Collection<Position>>();;
	/** interpolated transitions */
	LinkedHashMap<Integer, Position> interpolated = new LinkedHashMap<Integer, Position>();
	/** Last position before the one to calculate. */
	Position lastPos;
	/** Next position to calculate, (state by default that is a template). */
	Position nextPos;
	/** The target position is any. */
	Position targetPos = null;
	/** minute to calculate. */
	int minuteToCalc = 0;
	/** nb sec to reach next position. */
	double deltaSeconde = -7.1;
	double deltaSecCheck = -7.1;
	int deltaMinute = -7;
	/** for scoring. */
	double[] currentPreviousRevBga = null;
	double[] currentDangerCounter = null;
	int[] currentDangerCounterRev = null;

	/** if true then use positions map. */
	public boolean enhanceMode = false;

	protected AtomicInteger nbCaculationDone = new AtomicInteger(0);

	/** (beware of multi-threading). */
	static DecimalFormat FORMAT_SCORE = new DecimalFormat("0.00");

	public Position createInitialPosition(int initialMinute) {
		// if (!positions.isEmpty()) {
		// throw new RuntimeException("List of positions must be empty!");
		// }
		// this inital minute can be negative
		minuteToCalc = initialMinute;
		Position position = new Position();
		position.setMinute(initialMinute);
		StaticModel staticModel = new StaticModel();
		// yaw
		// yaw =
		staticModel.getInitialOrientation(beta);
		LOGGER.info("beta={}, yaw={}", beta, yaw);
		// LOGGER.warn("bug to correct : initialMinute % 92");
		position.setState(staticModel.getStateAtMinute(initialMinute % 92));
		position.transitionOk = true;
		lastPos = position;
		this.calculateScore(issCalculator, position);
		position.cumulateScore();
		LOGGER.info("intial score for {} = {}", initialMinute, FORMAT_SCORE.format(position.getScore()));
		// positions.put(initialMinute, position);
		return position;

	}
	
	/**
	 * 
	 * @param minute
	 */
	public void calculatePosition(int minute) {
		LOGGER.info("================================== calculatePosition minute={}", minute);
		initBeforeMinuteCalculation();
		if (positions.isEmpty()) {
			// init with static model
			positions.put(minute, createInitialPosition(minute));
			return;
		}
		// a way access last position and determine deltaMinute : nextMinute contain last minute calculated
		int minBefore = minuteToCalc;
		deltaMinute = minute - minBefore;
		deltaSeconde = deltaMinute * 60.0 * TIME_REDUCER;

		lastPos = positions.get(minBefore);
		assert lastPos != null;

		// the new position to optimise
		nextPos = new Position();
		nextPos.origin = Origin.PROJECTION;
		nextPos.minute = minute;
		minuteToCalc = minute;


		// init from last state considering the delta
		nextPos.initState(lastPos, deltaMinute, 0);
		// iterate on 10*2 positions
		for (int numRot = 0; numRot < NB_ROTERS; numRot++) {
			optimiseNextPosition(numRot);
		}
		// again 2 first main roters
		// optimiseNextPosition(0);
		// optimiseNextPosition(1);

		LOGGER.info("for minute {} , {}", minute, nextPos);
		// store next position
		positions.put(minute, nextPos);
	}

	private static final double MINIMIZE = 10.0 * EPS;

	private void optimiseNextPosition(int numRot) {
		LOGGER.debug("optimiseNextPosition for numRot {}", numRot);

		// shift for next reduction
		nextPos.shift = AngleUtil.getShift(lastPos.getAngle(numRot), nextPos.getAngle(numRot));

		// List<Position> allNewPos = new ArrayList<Position>();
		Map<Double, Position> mapNewPos = new HashMap<Double, Position>();
		// allNewPos.add(nextPos);
		mapNewPos.put(nextPos.shift, nextPos);

		// new pos to evaluate


		// calculate next max and min positions
		double maxSpeed = getMaxSpeed(numRot);
		double maxAcc = getMaxAcc(numRot);
		// not to be out of limits:
		if (deltaSeconde > 70.0) {
			// we will have to interpolate, so minimize speeds
			maxSpeed -= MINIMIZE;
			maxAcc -= MINIMIZE;
		}
		LOGGER.debug("maxSpeed={}, maxAcc={}", maxSpeed, maxAcc);
		double currentSpeed = lastPos.getSpeed(numRot);
		double[] nextShiftSpeed;

		// try to accelerate max
		Position maximPos = new Position(nextPos);
		maximPos.origin = Origin.MAX;
		nextShiftSpeed = calculateNextPos(currentSpeed, maxSpeed, maxAcc, deltaSeconde);
		// if (nextAngleSpeed[0] != 0.0) {
		addNewPos(numRot, mapNewPos, nextShiftSpeed, maximPos);

		// }

		// try to decelerate max
		Position minimPos = new Position(nextPos);
		minimPos.origin = Origin.MAX;
		nextShiftSpeed = calculateNextPos(currentSpeed, maxSpeed, -maxAcc, deltaSeconde);
		addNewPos(numRot, mapNewPos, nextShiftSpeed, minimPos);

		// if (nextMinute == 44) {
		// LOGGER.warn("case to debug");
		// }
		PathOptimizer opt = PathOptimizer.getPathOptimizer(numRot);

		nextShiftSpeed = new double[2];
		{
			Position interPos = new Position(nextPos);
			interPos.origin = Origin.MID2;

		// medium speed
		nextShiftSpeed[1] = (maximPos.getSpeed(numRot) + minimPos.getSpeed(numRot)) / 2.0;
		nextShiftSpeed[0] = opt.calculateSimpleShift(currentSpeed, nextShiftSpeed[1], deltaSeconde);
		addNewPos(numRot, mapNewPos, nextShiftSpeed, interPos);
		}
		// +1/8
		{
			Position interPos = new Position(nextPos);
			interPos.origin = Origin.MID8;
			// medium speed
			nextShiftSpeed[1] = (5.0 * maximPos.getSpeed(numRot) + 3.0 * minimPos.getSpeed(numRot)) / 8.0;
			nextShiftSpeed[0] = opt.calculateSimpleShift(currentSpeed, nextShiftSpeed[1], deltaSeconde);
			addNewPos(numRot, mapNewPos, nextShiftSpeed, interPos);
		}
		// -1/8
		{
			Position interPos = new Position(nextPos);
			interPos.origin = Origin.MID8;
			// medium speed
			nextShiftSpeed[1] = (3.0 * maximPos.getSpeed(numRot) + 5.0 * minimPos.getSpeed(numRot)) / 8.0;
			nextShiftSpeed[0] = opt.calculateSimpleShift(currentSpeed, nextShiftSpeed[1], deltaSeconde);
			addNewPos(numRot, mapNewPos, nextShiftSpeed, interPos);
		}
		

		Collection<Position> population = new LinkedList<Position>(mapNewPos.values());

		{
			// case where 8 BGA did not rote
			Position interPos = new Position(nextPos);
			interPos.origin = Origin.ZERO;
			for (int iBga = 0; iBga < 8; iBga++) {
				int numRotZero = iBga + 2;
				interPos.setAngle(numRotZero, lastPos.getAngle(numRotZero));
				interPos.setSpeed(numRotZero, 0.0);
			}
			population.add(interPos);
		}

		// List<Position> allNewPos = new ArrayList<Position>(Arrays.asList(new Position[] { nextPos, maximPos, minimPos
		// }));
		// nextPos = keepBestPosition(allNewPos, numRot, 1);
		removeImpossibleTransitions(population, true);
		calculateScores(population);
		nextPos = keepBestScores(population);
		

		// check rendez-vous is possible
		if (92 - minuteToCalc < 20) {
			LOGGER.debug("check rendez-vous");
			Position pos0 = positions.get(0);
			// if (nextMinute == 84 && numRot == 1) {
			// LOGGER.info("debug");
			// }
			if (opt.canMakeTrans(nextPos.getAngle(numRot), nextPos.getSpeed(numRot), pos0.getAngle(numRot), pos0.getSpeed(numRot),
					(92 - minuteToCalc) * 60.0)[0] != 1.0) {
				LOGGER.info("Can not optimise, need to interpolate to rendez-vous mn={} rot={}", minuteToCalc, numRot);
				Position posBefore = getLastOptimize(minuteToCalc, numRot);
				double[] angleAndSpeed;
				angleAndSpeed = opt.interpolate(posBefore.getAngle(numRot), posBefore.getSpeed(numRot), pos0.getAngle(numRot),
						pos0.getSpeed(numRot), (92 - posBefore.minute) * 60.0, (minuteToCalc - posBefore.minute) * 60.0);
				nextPos.setAngle(numRot, angleAndSpeed[0]);
				nextPos.setSpeed(numRot, angleAndSpeed[1]);
				nextPos.setInterpolated(numRot, true);
				nextPos.origin = Origin.INTERPOL;


				// check transition is possible with next :
				if (opt.canMakeTrans(angleAndSpeed[0], angleAndSpeed[1], pos0.getAngle(numRot), pos0.getSpeed(numRot),
						(92 - minuteToCalc) * 60.0)[0] != 1.0) {
					throw new RuntimeException("impossible transition to finish");
				}
			}

		}

		// check calculated transition is possible
		if (!opt.canMakeTrans(AngleUtil.getShift(lastPos.getAngle(numRot), nextPos.getAngle(numRot)), lastPos.getSpeed(numRot),
				nextPos.getSpeed(numRot), deltaSeconde)) {
			LOGGER.warn("numRot {}", numRot);
			LOGGER.warn("{} {} {} {}", AngleUtil.getShift(lastPos.getAngle(numRot), nextPos.getAngle(numRot)),
					lastPos.getSpeed(numRot), nextPos.getSpeed(numRot), deltaSeconde);
			// throw new RuntimeException("impossible transition at minute " + nextMinute);
		}
	}

	protected void calculateScores(Collection<Position> population) {
		LOGGER.debug("calculating {} scores", population.size());

		Collection<ICalculateIss> lstCalc = parallelMgr.getAllAvailableCalculators();
		final Iterator<ICalculateIss> it = lstCalc.iterator();
		// final SynchronousQueue<ICalculateIss> queue = new SynchronousQueue<ICalculateIss>();
		// queue.addAll(lstCalc);
		ThreadFactory threadFactory = new ThreadFactory() {
			public Thread newThread(Runnable r) {
				LOGGER.debug("newThread");
				ThreadCalculator t = new ThreadCalculator(r);
				t.setCalculator(it.next());
				return t;
			}
		};
		ExecutorService executor = Executors.newFixedThreadPool(lstCalc.size(), threadFactory);

		for (final Position position : population) {
			Runnable worker = new Runnable() {

				public void run() {
					// calculateScore(issCalculator, position);
					// ThreadLocal<>
					ThreadCalculator t = (ThreadCalculator) Thread.currentThread();
					calculateScore(t.getCalculator(), position);
				}
			};
			executor.execute(worker);
			// calculateScore(issCalculator, position);
		}
		// This will make the executor accept no new threads
		// and finish all existing threads in the queue
		executor.shutdown();

		// Wait until all threads have completed
		while (!executor.isTerminated()) {
			try {
				Thread.sleep(1);
			} catch (InterruptedException e) {
				LOGGER.error("", e);
			}
		}

		// now calculate score for children in advance
		if (currentNbAdcance > 1) {
			LOGGER.info("calculating scores of children in advance");
			for (Position position : population) {
				Position childA = position.childAdvance;
				if (childA == null) {
					LOGGER.error("position=" + position);
					throw new RuntimeException("Missing children for advance " + currentNbAdcance);
				}
				calculateScore(issCalculator, childA);
			}
		}
	}

	private double[] getDangerCounterFromPrevious19(Position posDepart) {
		if (currentDangerCounter != null) {
			return currentDangerCounter;
		}
		double[] dangerCounter = new double[8];
		Arrays.fill(dangerCounter, 0.0);
		// take 19 previous calculations
		int nbExtract = 20;
		Collection<Position> previous19Col = extractParents(posDepart, posDepart.getMinute() - nbExtract);
		if (!previous19Col.isEmpty()) {

			for (Position position : previous19Col) {
				for (int iBga = 0; iBga < dangerCounter.length; iBga++) {
					dangerCounter[iBga] += position.dangerCounter[iBga];
				}
			}
			if (previous19Col.size() != nbExtract) {
				double multiply = ((double) nbExtract) / previous19Col.size();
				if (multiply != 1.0) {
					// LOGGER.info("multiply={}", multiply);
					for (int iBga = 0; iBga < dangerCounter.length; iBga++) {
						dangerCounter[iBga] *= multiply;
					}
				}
			}

		}
		currentDangerCounter = dangerCounter;
		return dangerCounter;
	}

	private Collection<Position> extractPosition(int minBegin, int minEnd) {
		Collection<Position> extraction = new LinkedList<Position>();
		for (int i = minBegin; i <= minEnd; i++) {
			Position aPos = positions.get(i);
			if (aPos != null) {
				extraction.add(aPos);
			}
		}
		return extraction;
	}

	/**
	 * oldestMinute can be negative
	 * 
	 * @param posDepart
	 * @param oldestMinute
	 * @return
	 */
	List<Position> extractParents(Position posDepart, int oldestMinute) {
		List<Position> extraction = new LinkedList<Position>();
		Position curPos = posDepart.parent;
		if (enhanceMode) {
			// int nb = 0;
			boolean turnDone = false;
			int lastMinuteAdded = 100;
			while (curPos != null) {
				if ((curPos.getMinute() > lastMinuteAdded) && !turnDone) {
					oldestMinute += 92;
					turnDone = true;
				}
				// if ((curPos.getMinute() == 0) && !turnDone) {
				// oldestMinute += 92;
				// turnDone = true;
				// }
				// if ((curPos.getMinute() > posDepart.getMinute()) && !turnDone) {
				// oldestMinute += 92;
				// turnDone = true;
				// }

				if (curPos.getMinute() >= oldestMinute) {
					lastMinuteAdded = curPos.getMinute();
					extraction.add(curPos);
					// nb++;
				} else {
					LOGGER.debug("end loop");
					break;
				}

				// if (curPos.getMinute() < posDepart.getMinute()) {
				// if ((curPos.getMinute() < oldestMinute)) {
				// break;
				// }
				// } else {
				// if ((curPos.getMinute() - 92 < oldestMinute)) {
				// break;
				// }
				// }
				// extraction.add(curPos);
				// next
				curPos = curPos.parent;
			}

		} else {
			// int nb = 0;
			while (curPos != null && curPos.getMinute() >= oldestMinute) {
				extraction.add(curPos);
				// nb++;
				// next
				curPos = curPos.parent;
			}
		}
		// reverse list order to have older at first position
		Collections.reverse(extraction);
		return extraction;
	}

	Position extractParent(Position posDepart, int minuteRequested) {

		Position curPos = posDepart.parent;
		while (curPos != null && curPos.getMinute() > minuteRequested) {
			// next
			curPos = curPos.parent;
		}

		return curPos;
	}

	protected void removeImpossibleTransitions(Collection<Position> population, boolean remove) {
		int size = population.size();
		Iterator<Position> it = population.iterator();
		while (it.hasNext()) {
			Position position = it.next();
			if (position.transitionOk == null) {
				position.transitionOk = transitionOkForPosition(position);
			}
			if (!position.transitionOk && remove) {
				LOGGER.debug("remove {}", position);
						it.remove();
			}
		}
		if (population.isEmpty()) {
			LOGGER.error("no more population!!!");
		}
		if (remove) {
			LOGGER.debug("removed {}", size - population.size());
		}
	}

	protected Position keepBestScores(Collection<Position> population) {
		if (population.isEmpty()) {
			LOGGER.error("no more elements !");
		}
		SortedMap<Double, Position> sorted = new TreeMap<Double, Position>();
		for (Position position : population) {
			sorted.put(position.getScore(), position);
		}
		return sorted.get(sorted.lastKey());
		// return sorted.get(sorted.firstKey());
	}

	protected final boolean transitionOkForPosition(Position position) {
		if (!PathOptimizer.transitionOkForPosition(lastPos, position)) {
			return false;
		}
		// for (int numRot = 0; numRot < NB_ROTERS; numRot++) {
		// PathOptimizer opt = PathOptimizer.getPathOptimizer(numRot);
		// double currentAngle = lastPos.getAngle(numRot);
		// if (currentAngle < 0.0 || currentAngle >= 360.0) {
		// throw new RuntimeException("Invalid angle " + currentAngle + " for rot " + numRot + " in " + position);
		// }
		// if (!opt.canMakeTransBool(currentAngle, lastPos.getSpeed(numRot), position.getAngle(numRot),
		// position.getSpeed(numRot), deltaSecCheck)) {
		// // LOGGER.info("{}, {}, {}, {}, {}, ", currentAngle, currentPos.getSpeed(numRot),
		// // position.getAngle(numRot),
		// // position.getSpeed(numRot), deltaSecCheck);
		// return false;
		// }
		// }
		if (targetPos != null) {
			// Position pos0 = positions.get(0);
			// Position pos0 = targetPos;
			if (!PathOptimizer.transitionOkForPosition(position, targetPos)) {
				return false;
			}
			// for (int numRot = 0; numRot < NB_ROTERS; numRot++) {
			// PathOptimizer opt = PathOptimizer.getPathOptimizer(numRot);
			// if (!opt.canMakeTransBool(position.getAngle(numRot), position.getSpeed(numRot), pos0.getAngle(numRot),
			// pos0.getSpeed(numRot), (92 - minuteToCalc) * 60.0)) {
			// return false;
			// }
			// }
		}
		return true;
	}


	private void addNewPos(int numRot, Map<Double, Position> mapNewPos, double[] nextShiftSpeed, Position newPos) {
		newPos.setAngle(numRot, AngleUtil.addAngle(lastPos.getAngle(numRot), nextShiftSpeed[0]));
		newPos.shift = nextShiftSpeed[0];
		newPos.setSpeed(numRot, nextShiftSpeed[1]);
		if (mapNewPos.containsKey(newPos.shift)) {
			LOGGER.debug("already {}", newPos.shift);
		} else {
			mapNewPos.put(newPos.shift, newPos);
		}
	}

	private Position getLastOptimize(int minute, int numRot) {
		Position posBefore = null;
		for (int i = minute - 1; i >= 0; i--) {
			posBefore = positions.get(i);
			if ((posBefore != null) && (!posBefore.isInterpolated(numRot))) {
				// first non interpolated in the past
				posBefore.minute = i;
				return posBefore;
			}
		}
		return posBefore;
	}

	private Position getNextOptimized(int minute, int numRot) {
		Position posSearched = null;
		for (int i = minute + 1; i <= 92; i++) {
			// 92 means 0
			posSearched = positions.get(i % 92);
			if ((posSearched != null) && (!posSearched.isInterpolated(numRot))) {
				// first non interpolated in the future
				posSearched.minute = i;
				return posSearched;
			}
		}
		return posSearched;
	}

	/**
	 * 
	 * @param currentSpeed
	 *            speed at begining
	 * @param maxSpeed
	 *            maximum autorised speed
	 * @param acc
	 * @param deltaSeconde
	 * @return double[] with deltaAngle and nextSpeed
	 */
	public static final double[] calculateNextPos(double currentSpeed, double maxSpeed, double acc, double deltaSeconde) {
		assert maxSpeed > 0.0;
		double deltaAngle;
		double nextSpeed;
		double nextSpeedMax = currentSpeed + acc * deltaSeconde;
		// double averageSpeed;
		double secInter = 0.0;
		if (nextSpeedMax >= maxSpeed) {
			nextSpeed = maxSpeed;
			// maximPos.setSpeed(numRot, maxSpeed);
			// maximum autorised speed is reached
			// need to have intermediate point
			secInter = (maxSpeed - currentSpeed) / acc;
			assert secInter >= 0.0;
			assert secInter <= deltaSeconde;

			// deltaAngle = currentSpeed * secInter + (acc * secInter * secInter / 2.0);
			// deltaAngle += maxSpeed * (deltaSeconde - secInter);

		} else if (nextSpeedMax <= -maxSpeed) {
			nextSpeed = -maxSpeed;
			secInter = (-maxSpeed - currentSpeed) / acc;
			// if (!(secInter > 0.0)) {
			// LOGGER.warn("secInter={}", secInter);
			// }
			assert secInter >= 0.0;
			assert secInter <= deltaSeconde;
			
		} else {
			nextSpeed = nextSpeedMax;

			// // equivalent to :
			secInter = deltaSeconde;
			// // maximPos.setSpeed(numRot, nextSpeedMax);
			// deltaAngle = currentSpeed * deltaSeconde + (acc * deltaSeconde * deltaSeconde / 2.0);
		}
		deltaAngle = currentSpeed * secInter + (acc * secInter * secInter / 2.0);
		deltaAngle += nextSpeed * (deltaSeconde - secInter);

		if (deltaAngle > 180.0) {
			LOGGER.debug("deltaAngle max={} is too big for a good simulation, maxSpeed={}", deltaAngle, maxSpeed);
			deltaAngle = 180.0;
			// speed ?
		} else if (deltaAngle < -180.0) {
			LOGGER.debug("deltaAngle min={} is too big for a good simulation, maxSpeed={}", deltaAngle, maxSpeed);
			// just a little bit not to have same as positive
			deltaAngle = -179.99;
			// speed ?
		}

		return new double[] { (deltaAngle), nextSpeed };
	}


	protected final double calculateScore(ICalculateIss remoteCalculator, Position position) {
		if (position.getScore() != Position.UNSET) {
			LOGGER.debug("Score already calculated");
			return position.getScore();
		}
		nbCaculationDone.incrementAndGet();
		// LOGGER.info("caculating score minute {}", position);

		// Inputs:
		// - Frame ID. 0-based ID of the frame.
		// - Alpha angle. This and all subsequent angles are in degrees.
		// Thus at time t, 0 <= t < NUM_STATES, alpha angle is
		// equal to t / NUM_STATES * 360.0.
		// - Beta angle.
		// - Yaw angle.
		// - 2 SARJ angles.
		// - 8 BGA angles (in order: 1A, 2A, 3A, 4A, 1B, 2B, 3B, 4B.
		// Outputs:
		// - 8 cos(theta) values used for power calculation
		//
		// - NUM_STRINGSx8 shadow fractions (from 0.0 to 1.0) of individual strings
		// (first NUM_STRINGS values for 1A, then for 2A, then for 3A and so on).
		//
		// - NUM_LONGERONSx8 shadow fractions (from 0.0 to 1.0) of individual
		// longerons (first NUM_LONGERONS values for 1A, then for 2A, then for 3A and so on).

		double[] input = new double[14];
		// cell 0 only take positive minute and is not used
		// input[0] = nextMinute;
		input[0] = 0;
		input[1] = IssUtil.alphaForMinute(position.getMinute());
		input[2] = beta;
		if (position.tempYaw == Position.UNSET) {
			input[3] = yaw;
		} else {
			input[3] = position.tempYaw;
		}

		for (int numRot = 0; numRot < 10; numRot++) {
			input[numRot + 4] = position.getAngle(numRot);
		}
		// call to Nasa code
		double[] output = null;
		try {
			output = remoteCalculator.evaluateSingleState(input);
		} catch (RemoteException e) {
			LOGGER.error("", e);
		}

		double[] previousRevBga = getPreviousRevBga(position);
		// double[] dangerCounter = getDangerCounterFromPrevious19(position);

		/*
		 * output is:<br>
		 * The cosine of the angle between the blanket normal and the vector to the sun for each SAW (8 values). The
		 * shadow fraction of each string (8 SAWs x 2 blankets x 41 strings = 656 values). The shadow fraction of each
		 * longeron (8 SAWs x 4 longerons = 32 values).
		 */
		double score = calc.calculate(output, lastPos, position, currentDangerCounterRev, previousRevBga, deltaSeconde);
		if (!position.transitionOk) {
			score *= 0.5;
		}
		position.setScore(score);
		return score;

	}

	protected void initBeforeMinuteCalculation() {
		currentPreviousRevBga = null;
		currentDangerCounter = null;
	}

	/** assumption on previous turns. */
	private static final double AVERAGE_TURN_PER_MN = 80.0 / 92.0 * 1.0;

	synchronized private double[] getPreviousRevBga(Position posDepart) {
		if (currentPreviousRevBga != null) {
			return currentPreviousRevBga;
		}
		double[] previousRevBga = new double[8];
		Arrays.fill(previousRevBga, 0.0);

		currentDangerCounterRev = new int[8];
		Arrays.fill(currentDangerCounterRev, 0);

		int nbExtract = 91;
		int oldestMinute = posDepart.getMinute() - nbExtract;
		List<Position> previousPos = extractParents(posDepart, oldestMinute);
		LOGGER.debug("getPreviousRevBga,  previousPos.size()={}", previousPos.size());
		if (!previousPos.isEmpty()) {
			Iterator<Position> it = previousPos.iterator();
			Position lastPos = it.next();
			while (it.hasNext()) {
				Position curPos = it.next();
				int prevDeltaMinute = 1;
				if (curPos.parent != null) {
					prevDeltaMinute = curPos.parent.getDelatMinuteToPos(curPos);
				}
				for (int iBga = 0; iBga < previousRevBga.length; iBga++) {
					// if (lastPos.getMinute() < 0) {
					// // negative minutes will not be scored by contest
					// previousRevBga[iBga] += Math.abs(AngleUtil.getShift(lastPos.getAngle(iBga + 2),
					// curPos.getAngle(iBga + 2))
					// / CalcPower.DIVISOR_FOR_NEG_REV);
					// } else {
					previousRevBga[iBga] += Math.abs(AngleUtil.getShift(lastPos.getAngle(iBga + 2), curPos.getAngle(iBga + 2)));
					currentDangerCounterRev[iBga] += curPos.dangerCounter[iBga] * prevDeltaMinute;
					if (currentDangerCounterRev[iBga] < 0) {
						currentDangerCounterRev[iBga] = 0;
					}
					// }
				}
				lastPos = curPos;
			}

			if (!enhanceMode) {
			// if we miss some minute, we assume an average turn
			if (oldestMinute < previousPos.get(0).getMinute()) {
				double toAdd = AVERAGE_TURN_PER_MN * (previousPos.get(0).getMinute() - oldestMinute);
				LOGGER.info("getPreviousRevBga toAdd={}", toAdd);
					// LOGGER.info("multiply={}", multiply);
					for (int iBga = 0; iBga < previousRevBga.length; iBga++) {
					previousRevBga[iBga] += toAdd;
					}
			}
			}

		}

		currentPreviousRevBga = previousRevBga;
		return previousRevBga;
	}

	protected static double getMaxSpeed(int numRot) {
		if (numRot < 2) {
			// 0.15 deg/s
			// from 0 to max in 30s
			// from -max to max in 60s
			return ConstraintsChecker.MAX_SARJ_SPEED;
		} else {
			return ConstraintsChecker.MAX_BGA_SPEED;
		}
	}

	protected static double getMaxAcc(int numRot) {
		if (numRot < 2) {
			// 0.0050
			return ConstraintsChecker.MAX_SARJ_ACC;
		} else {
			// 0.01
			return ConstraintsChecker.MAX_BGA_ACC;
		}
	}

	// private static final PathOptimizer OPT_SARG = new PathOptimizer(ConstraintsChecker.MAX_SARJ_SPEED,
	// ConstraintsChecker.MAX_SARJ_ACC);
	// private static final PathOptimizer OPT_BGA = new PathOptimizer(ConstraintsChecker.MAX_BGA_SPEED,
	// ConstraintsChecker.MAX_BGA_ACC);
	//
	// protected static PathOptimizer getPathOptimizer(int numRot) {
	// if (numRot < 2) {
	// // 0.0050
	// return OPT_SARG;
	// } else {
	// // 0.01
	// return OPT_BGA;
	// }
	// }

	// === getters and setters ===

	public double getBeta() {
		return beta;
	}

	public void setBeta(double beta) {
		this.beta = beta;
	}

	public double getYaw() {
		return yaw;
	}

	public void setYaw(double yaw) {
		this.yaw = yaw;
	}

	public Map<Integer, Position> getPositions() {
		return positions;
	}

	/**
	 * Save result in CSV file in
	 * 
	 * @param mnPerStep
	 * 
	 * @throws IOException
	 */
	public void saveResultInCsv(String message) throws IOException {
		int nbCalculated = extractPosition(0, 92).size();
		if (nbCalculated < 1) {
			LOGGER.error("Not enough element to build CSV file! {}", nbCalculated);
		}
		if (nbCalculated < 92) {
			// interposlate missings
			interpolateAllMissing();
		}
		String csvFileName = CsvIss.DIR_BASE_NAME + "/try/" + getSimulationName() + "s.csv";
		// CsvIss.getCsvTryName(beta, yaw);
		LOGGER.info("saving in {}", csvFileName);

		CSVWriter writer = new CSVWriter(new FileWriter(csvFileName), ',', CSVWriter.NO_QUOTE_CHARACTER);
		// firstLine.add("#firstLine");
		writer.writeNext(new String[] { "#firstLine. " + message });
		// writer.setWriter(new FileWriter(csvFileName));

		String[] lineStr = new String[22];
		lineStr[0] = Double.toString(beta);
		lineStr[1] = Double.toString(yaw);

		// format number in CSV
		NumberFormat format = NumberFormat.getInstance(Locale.US);
		format.setMaximumFractionDigits(10);
		format.setGroupingUsed(false);

		for (int minute = 0; minute < 92; minute++) {
			double[] line = getStateAtMinute(minute);
			// translate table
			for (int i = 0; i < line.length; i++) {
				double d = line[i];
				// lineStr[i + 2] = Double.toString(d);
				lineStr[i + 2] = format.format(d);
			}
			writer.writeNext(lineStr);
		}

		// writer.

		writer.flush();
		writer.close();


	}

	/**
	 * Persit the simulation in order to reload it later.
	 * 
	 * @throws IOException
	 */
	public void saveSimulation() throws IOException {
		int nbCalculated = extractPosition(0, 92).size();
		if (nbCalculated < 1) {
			LOGGER.error("Not enough element to build CSV file! {}", nbCalculated);
			return;
		}
		String simulationName = getSimulationName();
		File simulFile = new File(CsvIss.DIR_BASE_NAME + "/simu/" + simulationName + ".csv");
		// String csvFileName = CsvIss.DIR_BASE_NAME + "/simu/" + getSimulationName() + "s.csv";
		// CsvIss.getCsvTryName(beta, yaw);
		LOGGER.info("saving in {}", simulFile);

		CSVWriter writer = new CSVWriter(new FileWriter(simulFile), ',', CSVWriter.NO_QUOTE_CHARACTER);
		// firstLine.add("#firstLine");
		// writer.writeNext(new String[] { "#firstLine. " + message });
		// writer.setWriter(new FileWriter(csvFileName));

		String[] lineStr = new String[2];
		lineStr[0] = Double.toString(beta);
		lineStr[1] = Double.toString(yaw);

		writer.writeNext(lineStr);

		// format number in CSV
		NumberFormat format = NumberFormat.getInstance(Locale.US);
		format.setMaximumFractionDigits(10);
		format.setGroupingUsed(false);

		// the last for distance from 0 ?

		for (int minute = 0; minute < 92; minute++) {
			Position position = positions.get(minute);
			if (position != null) {
				lineStr = new String[1 + 20 + 8 + 1];
				lineStr[0] = Integer.toString(minute);
				double[] lineState = position.getState();
				// translate table
				for (int i = 0; i < lineState.length; i++) {
					double d = lineState[i];
					lineStr[i + 1] = Double.toString(d);
					// lineStr[i + 1] = format.format(d);
				}
				int[] danger = position.dangerCounter;
				for (int i = 0; i < danger.length; i++) {
					int d = danger[i];
					lineStr[i + 21] = Integer.toString(d);
				}
				lineStr[29] = format.format(position.getScore());
				writer.writeNext(lineStr);
			}

		}

		// writer.

		writer.flush();
		writer.close();

		LOGGER.info("csv simu created");
	}

	public void loadSimulation(String simulationName) throws IOException {
		File simulFile = new File(CsvIss.DIR_BASE_NAME + "/simu/" + simulationName + ".csv");
		LOGGER.info("reading from {}", simulFile);
		CSVReader reader = new CSVReader(new FileReader(simulFile), ',', CSVWriter.NO_QUOTE_CHARACTER);
		String[] lineStr = reader.readNext();
		beta = Double.parseDouble(lineStr[0]);
		yaw = Double.parseDouble(lineStr[1]);
		Position prevPos = null;
		Position firstPos = null;
		while ((lineStr = reader.readNext()) != null) {
			int minute = Integer.parseInt(lineStr[0]);
			Position position = new Position();
			position.origin = Origin.CSV;
			position.setMinute(minute);
			position.parent = prevPos;
			position.transitionOk = true;
			double[] state = new double[20];
			for (int i = 0; i < state.length; i++) {
				state[i] = Double.parseDouble(lineStr[i + 1]);
			}
			position.setState(state);

			int[] danger = new int[8];
			for (int i = 0; i < 8; i++) {
				danger[i] = Integer.parseInt(lineStr[i + 21]);
			}
			position.dangerCounter = danger;

			position.setScore(Double.parseDouble(lineStr[29]));

			position.cumulateScore();

			if (firstPos != null) {
				firstPos = position;
			}
			// positions.put(minute, position);
			// for next
			prevPos = position;
		}
		reader.close();

		fillMapMinutes(prevPos);

		if (!this.validateAllTransitions()) {
			throw new IssException("Invalid loaded model");
		}
	}

	public void fillMapMinutes(Position lastPosition) {
		positions = new TreeMap<Integer, Position>();
		positions.put(lastPosition.getMinute(), lastPosition);
		Position currentPos = lastPosition.parent;
		Position firstPos = null;
		while (currentPos != null) {
			positions.put(currentPos.getMinute(), currentPos);
			firstPos = currentPos;
			// for next
			currentPos = currentPos.parent;
		}
		// the last is the parent of the first
		firstPos.parent = lastPosition;
		LOGGER.info("fillMapMinutes, positions.size()={}", positions.size());
	}

	public boolean validateAllTransitions() {
		int nbChecks = 0;
		Map<Integer, Position> mapPositions = this.getPositions();
		Set<Integer> allKeys = mapPositions.keySet();
		Iterator<Integer> it = allKeys.iterator();
		int firstMinute = 0;
		int min1 = firstMinute;
		while (it.hasNext()) {
			int min2 = it.next();
			if (min2 >= 0) {
				if (!PathOptimizer.transitionOkForPosition(mapPositions.get(min1), mapPositions.get(min2))) {
					LOGGER.warn("impossible from {} to {}", min1, min2);
					return false;
				}
				nbChecks++;
				// for next
				min1 = min2;
			}
		}
		// the last transition to validate
		if (!PathOptimizer.transitionOkForPosition(mapPositions.get(min1), mapPositions.get(firstMinute))) {
			LOGGER.warn("last impossible from {} to {}", min1, firstMinute);
			return false;
		}
		nbChecks++;
		LOGGER.info("nbChecks={}", nbChecks);
		return true;
	}

	public boolean interpolateAllMissing() {
		int nbInterpol = 0;
		Map<Integer, Position> mapPositions = this.getPositions();
		Set<Integer> allKeys = new TreeSet<Integer>(mapPositions.keySet());
		Iterator<Integer> it = allKeys.iterator();
		int firstMinute = 0;
		int min1 = firstMinute;
		while (it.hasNext()) {

			int min2 = it.next();
			if (min2 > 1) {

			if (!PathOptimizer.transitionOkForPosition(mapPositions.get(min1), mapPositions.get(min2))) {
				LOGGER.warn("impossible from {} to {}", min1, min2);
				return false;
			}

			if (min1 < min2 - 1) {
				for (int minute = min1 + 1; minute < min2; minute++) {
					Position position = interpolatePosition(minute, mapPositions.get(min1), mapPositions.get(min2));
					positions.put(minute, position);
					nbInterpol++;
				}
			}

			// for next
			min1 = min2;

			}
		}
		// the last transition to validate
		if (!PathOptimizer.transitionOkForPosition(mapPositions.get(min1), mapPositions.get(firstMinute))) {
			LOGGER.warn("last impossible from {} to {}", min1, firstMinute);
			return false;
		}

		for (int minute = min1 + 1; minute < firstMinute + 92; minute++) {
			Position position = interpolatePosition(minute, mapPositions.get(min1), mapPositions.get(firstMinute));
			positions.put(minute, position);
			nbInterpol++;
		}
		LOGGER.info("nbInterpol={}", nbInterpol);
		return true;
	}

	private Position interpolatePosition(int minute, Position posBefore, Position posAfter) {
		Position position = new Position();
		position.setMinute(minute);
		//
		// Position posBefore = getLastOptimize(minute, numRot);
		// Position posAfter = getNextOptimized(minute, numRot);

		// nb from before to after
		// double deltaSecT = 60.0 * (posAfter.getMinute() - posBefore.getMinute());
		double deltaSecT = 60.0 * posBefore.getDelatMinuteToPos(posAfter);
		// double nbSecInter = 60.0 * (minute - posBefore.getMinute());
		double nbSecInter = 60.0 * posBefore.getDelatMinuteToPos(position);
		// LOGGER.info("interpolating {} with {} {}", minute, posBefore.getMinute(), posAfter.getMinute());

		for (int numRot = 0; numRot < 10; numRot++) {
			PathOptimizer opt = PathOptimizer.getPathOptimizer(numRot);

			double maxSpeed = getMaxSpeed(numRot);
			double maxAcc = getMaxAcc(numRot);

			double angleBefore = posBefore.getAngle(numRot);
			// delta angle total
			// double distAngle = AngleUtil.distance(angleBefore, posAfter.getAngle(numRot));

			double v1 = posBefore.getSpeed(numRot);
			double v2 = posAfter.getSpeed(numRot);

			// if (minute == 11) {
			// LOGGER.warn("to debug");
			// }
			double[] angleAndSpeed;
			try {
				angleAndSpeed = opt.interpolate(angleBefore, v1, posAfter.getAngle(numRot), v2, deltaSecT, nbSecInter);
			} catch (Exception e) {
				LOGGER.error("minute=" + minute + " " + numRot + " " + posBefore.minute + " " + posAfter.minute);
				throw new RuntimeException(e);
			}

			// if (v2 >= v1) {
			// angleAndSpeed = calculateAPath(t2, deltaSecT, tCalc, maxAcc, distAngle, v1, v2);
			// } else {
			// angleAndSpeed = calculateAPath(t2, deltaSecT, tCalc, -maxAcc, distAngle, v1, v2);
			// }
			// position.setAngle(numRot, AngleUtil.addAngle(angleBefore, angleAndSpeed[0]));

			position.setAngle(numRot, angleAndSpeed[0]);
			position.setSpeed(numRot, angleAndSpeed[1]);

			// position.setAngle(numRot, (angleBefore * distBefore + posAfter.getAngle(numRot) * distAfter)
			// / (distBefore + distAfter));
			// position.setSpeed(numRot, (v1 * distBefore + v2 * distAfter)
			// / (distBefore + distAfter));

			double angleCalculated = position.getAngle(numRot);
			{
				// check if transition is possible

				double[] ret = processTransition(angleBefore, v1, angleCalculated, position.getSpeed(numRot), maxSpeed, maxAcc, nbSecInter);
				if (ret[0] == 0.0) {
					LOGGER.error("impossible transition mn{} ret{} {} , angle before={} calc={}", minute, ret[0], ret[1], angleBefore,
							angleCalculated);
				}
			}

		}
		return position;
	}

	public void interpolateAllMissingOld() {
		int nbInterpolated = 0;
		for (int minute = 0; minute < 92; minute++) {
			Position position = positions.get(minute);
			if (position == null) {
				// need to interpolate with before and after positions
				position = new Position();
				position.setMinute(minute);
				for (int numRot = 0; numRot < 10; numRot++) {
					PathOptimizer opt = PathOptimizer.getPathOptimizer(numRot);

					Position posBefore = getLastOptimize(minute, numRot);
					Position posAfter = getNextOptimized(minute, numRot);

					// nb from before to after
					// double deltaSecT = 60.0 * (posAfter.getMinute() - posBefore.getMinute());
					double deltaSecT = 60.0 * posBefore.getDelatMinuteToPos(posAfter);
					// double nbSecInter = 60.0 * (minute - posBefore.getMinute());
					double nbSecInter = 60.0 * posBefore.getDelatMinuteToPos(position);
					LOGGER.debug("interpolating {} with {} {}", minute, posBefore.getMinute(), posAfter.getMinute());

					double maxSpeed = getMaxSpeed(numRot);
					double maxAcc = getMaxAcc(numRot);

					double angleBefore = posBefore.getAngle(numRot);
					// delta angle total
					// double distAngle = AngleUtil.distance(angleBefore, posAfter.getAngle(numRot));

					double v1 = posBefore.getSpeed(numRot);
					double v2 = posAfter.getSpeed(numRot);

					// if (minute == 11) {
					// LOGGER.warn("to debug");
					// }
					double[] angleAndSpeed;
					try {
						angleAndSpeed = opt.interpolate(angleBefore, v1, posAfter.getAngle(numRot), v2, deltaSecT, nbSecInter);
					} catch (Exception e) {
						LOGGER.error("minute=" + minute + " " + numRot + " " + posBefore.minute + " " + posAfter.minute);
						throw new RuntimeException(e);
					}

					// if (v2 >= v1) {
					// angleAndSpeed = calculateAPath(t2, deltaSecT, tCalc, maxAcc, distAngle, v1, v2);
					// } else {
					// angleAndSpeed = calculateAPath(t2, deltaSecT, tCalc, -maxAcc, distAngle, v1, v2);
					// }
					// position.setAngle(numRot, AngleUtil.addAngle(angleBefore, angleAndSpeed[0]));

					position.setAngle(numRot, angleAndSpeed[0]);
					position.setSpeed(numRot, angleAndSpeed[1]);

					// position.setAngle(numRot, (angleBefore * distBefore + posAfter.getAngle(numRot) * distAfter)
					// / (distBefore + distAfter));
					// position.setSpeed(numRot, (v1 * distBefore + v2 * distAfter)
					// / (distBefore + distAfter));

					double angleCalculated = position.getAngle(numRot);
					{
						// check if transition is possible

						double[] ret = processTransition(angleBefore, v1, angleCalculated, position.getSpeed(numRot), maxSpeed, maxAcc,
								nbSecInter);
						if (ret[0] == 0.0) {
							LOGGER.error("impossible transition mn{} ret{} {} , angle before={} calc={}", minute, ret[0], ret[1],
									angleBefore, angleCalculated);
						}
					}

					// check if transition is possible with previous
					// if (nbSecInter > 70.0) {
					// Position posLastInt = interpolated.get(minute - 1);
					// if (posLastInt == null) {
					// posLastInt = positions.get(minute - 1);
					// }
					// double[] ret = processTransition(posLastInt.getAngle(numRot), posLastInt.getSpeed(numRot),
					// angleCalculated,
					// position.getSpeed(numRot), maxSpeed, maxAcc, 60.0);
					// if (ret[0] == 0.0) {
					// LOGGER.error("impossible transition from previous interpolated mn{} ret{} {} , angle before={} calc={}",
					// minute, ret[0], ret[1], angleBefore, angleCalculated);
					// }
					// }

				}
				positions.put(minute, position);
				interpolated.put(minute, position);
				nbInterpolated++;
			}
		}
		LOGGER.info("nbInterpolated={}", nbInterpolated);
	}

	private double[] getStateAtMinute(int minute) {
		Position position = positions.get(minute);
		if (position == null) {
			LOGGER.error("no Position for minute {}", minute);
			// need to interpolate with before and after positions
			// position = new Position();
			// for (int numRot = 0; numRot < 10; numRot++) {
			// PathOptimizer opt = PathOptimizer.getPathOptimizer(numRot);
			//
			// Position posBefore = getLastOptimize(minute, numRot);
			// Position posAfter = getNextOptimized(minute, numRot);
			//
			// // nb from before to after
			// double deltaSecT = 60.0 * (posAfter.getMinute() - posBefore.getMinute());
			// double nbSecInter = 60.0 * (minute - posBefore.getMinute());
			// LOGGER.debug("interpolating {} with {} {}", minute, posBefore.getMinute(), posAfter.getMinute());
			//
			// double maxSpeed = getMaxSpeed(numRot);
			// double maxAcc = getMaxAcc(numRot);
			//
			// double angleBefore = posBefore.getAngle(numRot);
			// // delta angle total
			// // double distAngle = AngleUtil.distance(angleBefore, posAfter.getAngle(numRot));
			//
			// double v1 = posBefore.getSpeed(numRot);
			// double v2 = posAfter.getSpeed(numRot);
			//
			// // if (minute == 11) {
			// // LOGGER.warn("to debug");
			// // }
			// double[] angleAndSpeed;
			// try {
			// angleAndSpeed = opt.interpolate(angleBefore, v1, posAfter.getAngle(numRot), v2, deltaSecT, nbSecInter);
			// } catch (Exception e) {
			// LOGGER.error("minute=" + minute + " " + numRot + " " + posBefore.minute + " " + posAfter.minute);
			// throw new RuntimeException(e);
			// }
			//
			// // if (v2 >= v1) {
			// // angleAndSpeed = calculateAPath(t2, deltaSecT, tCalc, maxAcc, distAngle, v1, v2);
			// // } else {
			// // angleAndSpeed = calculateAPath(t2, deltaSecT, tCalc, -maxAcc, distAngle, v1, v2);
			// // }
			// // position.setAngle(numRot, AngleUtil.addAngle(angleBefore, angleAndSpeed[0]));
			//
			// position.setAngle(numRot, angleAndSpeed[0]);
			// position.setSpeed(numRot, angleAndSpeed[1]);
			//
			// // position.setAngle(numRot, (angleBefore * distBefore + posAfter.getAngle(numRot) * distAfter)
			// // / (distBefore + distAfter));
			// // position.setSpeed(numRot, (v1 * distBefore + v2 * distAfter)
			// // / (distBefore + distAfter));
			//
			// double angleCalculated = position.getAngle(numRot);
			// {
			// // check if transition is possible
			//
			// double[] ret = processTransition(angleBefore, v1, angleCalculated,
			// position.getSpeed(numRot), maxSpeed, maxAcc, nbSecInter);
			// if (ret[0] == 0.0) {
			// LOGGER.error("impossible transition mn{} ret{} {} , angle before={} calc={}", minute, ret[0], ret[1],
			// angleBefore,
			// angleCalculated);
			// }
			// }
			//
			// // check if transistion is possible with previous
			// if (nbSecInter > 70.0) {
			// Position posLastInt = interpolated.get(minute - 1);
			// if (posLastInt == null) {
			// posLastInt = positions.get(minute - 1);
			// }
			// double[] ret = processTransition(posLastInt.getAngle(numRot), posLastInt.getSpeed(numRot),
			// angleCalculated,
			// position.getSpeed(numRot), maxSpeed, maxAcc, 60.0);
			// if (ret[0] == 0.0) {
			// LOGGER.error("impossible transition from previous interpolated mn{} ret{} {} , angle before={} calc={}",
			// minute,
			// ret[0], ret[1], angleBefore,
			// angleCalculated);
			// }
			// }
			//
			// }
			//
			// interpolated.put(minute, position);
		}
		return position.getState();
	}



	/**
	 * Verifies whether a transition from (angle1, speed1) to (angle2, speed2) is possible within TIME_PER_STATE seconds
	 * assuming |speed| <= maxSpeed and |acceleration| <= maxAcc.<br>
	 * If it is possible, element 0 of return will be 1.0, otherwise 0.0.<br>
	 * Element 1 of return will contain the minimum amount of rotation necessary to make such transition. (This is only
	 * defined if transition is possible.)
	 */
	// =============================================
	double[] processTransition(double angle1, double speed1, double angle2, double speed2, double maxSpeed, double maxAcc, double deltaSec)
	// =============================================
	{
		// make sure speed1 >= 0, to reduce the number of cases
		if (speed1 < 0)
			return processTransition(-angle1, -speed1, -angle2, -speed2, maxSpeed, maxAcc, deltaSec);

		// determine angular shift we need to make
		double shift = angle2 - angle1;
		if (shift < -180.0)
			shift += 360.0;
		if (shift > 180.0)
			shift -= 360.0;

		// check validity
		double[] res = new double[] { 0.0, 0.0 };

		if (!canMakeTransition(shift, speed1, speed2, -maxSpeed, maxSpeed, maxAcc, deltaSec))
			return res;

		res[0] = 1.0;
		res[1] = Math.abs(shift);

		// check if some extra rotation is needed
		double pos, neg;
		if (speed2 >= 0) {
			// can we always keep speed positive?
			if (canMakeTransition(shift, speed1, speed2, 0, maxSpeed, maxAcc, deltaSec))
				return res;

			// minimize rotation at positive speeds
			double t1 = speed1 / maxAcc;
			double t2 = speed2 / maxAcc;
			pos = path(t1, 0, maxAcc) + path(t2, 0, maxAcc);
			neg = pos - shift;
		} else {
			double t1 = speed1 / maxAcc;
			double t2 = (-speed2) / maxAcc;
			pos = path(t1, 0, maxAcc);
			neg = path(t2, 0, maxAcc);
			double midShift = pos - neg;
			if (shift < midShift) {
				// minimize rotation at positive speeds
				neg = pos - shift;
			} else {
				// minimize rotation at negative speeds
				pos = neg + shift;
			}
		}

		res[1] = pos + neg;

		return res;
	}

	/**
	 * Verifies whether it's possible to make an angular shift of "shift" within TIME_PER_STATE seconds<br>
	 * if initial speed is "speed1", final speed is "speed2", minSpeed <= speed <= maxSpeed and |acceleration| <=
	 * maxAcc.
	 */
	// =============================================
	boolean canMakeTransition(double shift, double speed1, double speed2, double minSpeed, double maxSpeed, double maxAcc,
			double TIME_PER_STATE)
	// =============================================
	{
		// simple acceleration check
		if (Math.abs((speed2 - speed1) / TIME_PER_STATE) > maxAcc + EPS)
			return false;

		double minShift = 0.0, maxShift = 0.0;

		// find minimum possible angular shift
		double t1 = (speed1 - minSpeed) / maxAcc;
		double t2 = TIME_PER_STATE - (speed2 - minSpeed) / maxAcc;
		if (t1 <= t2) {
			minShift += path(t1, speed1, -maxAcc);
			minShift += path(t2 - t1, minSpeed, 0);
			minShift += path(TIME_PER_STATE - t2, minSpeed, maxAcc);
		} else {
			double t = (speed1 - speed2 + TIME_PER_STATE * maxAcc) / 2.0 / maxAcc;
			minShift += path(t, speed1, -maxAcc);
			minShift += path(TIME_PER_STATE - t, speed1 - maxAcc * t, maxAcc);
		}

		// find maximum possible angular shift
		t1 = (maxSpeed - speed1) / maxAcc;
		t2 = TIME_PER_STATE - (maxSpeed - speed2) / maxAcc;
		if (t1 <= t2) {
			maxShift += path(t1, speed1, maxAcc);
			maxShift += path(t2 - t1, maxSpeed, 0);
			maxShift += path(TIME_PER_STATE - t2, maxSpeed, -maxAcc);
		} else {
			double t = (speed2 - speed1 + TIME_PER_STATE * maxAcc) / 2.0 / maxAcc;
			maxShift += path(t, speed1, maxAcc);
			maxShift += path(TIME_PER_STATE - t, speed1 + maxAcc * t, -maxAcc);
		}

		// validate
		return (minShift <= shift && shift <= maxShift + EPS);
	}

	/** Returns path, given time, initial speed and acceleration. */
	// =============================================
	double path(double t, double v0, double a)
	// =============================================
	{
		return v0 * t + a * t * t / 2.0;
	}

}
